#include <stdio.h>
#include <stdint.h>
#include <iostream>
#include <vector>
#include <math.h>
#include "include/data_format_conversion.h"

using namespace std;

DataFormatConversion::DataFormatConversion() {

}

DataFormatConversion::~DataFormatConversion() {

}

void DataFormatConversion::conversion(std::vector<PointIn> &input, std::vector<PointInfo> &output) {
    int length = input.size();
    for (int j = 0; j < length; j++) {
        PointInfo tmp;
        tmp.pos_x = input[j].Range * cosf(input[j].Azimuth * pi / 180) * cosf(input[j].pitch * pi / 180);
        tmp.pos_y = input[j].Range * sinf(input[j].Azimuth * pi / 180) * cosf(input[j].pitch * pi / 180);
        tmp.pos_z = input[j].Range * sinf(input[j].pitch * pi / 180);
        tmp.SNR = input[j].SNR;
        tmp.r = input[j].Range;
        tmp.azi_angle = input[j].Azimuth;
        tmp.pitch_angle = input[j].pitch;
        tmp.doppler = input[j].Doppler;

        tmp.associate_count = 0;
        tmp.CPISeq = input[j].frame;
        tmp.correState = NONE;
        tmp.deletion_count = 0;
        tmp.livetime = 0;
        tmp.distance_increament = 0;
        if (tmp.doppler < 32) {
            //认为是靠近雷达，速度为负数
            tmp.velocity = (tmp.doppler - 256) * 0.366;   //不考虑速度方向
            tmp.biaozhi = -1;
        } else {
            //认为是远离雷达，速度为正数
            tmp.velocity = (256 - tmp.doppler) * 0.366;   //不考虑速度方向
            tmp.biaozhi = 1;
        }
        tmp.vel_x = tmp.velocity * cosf(input[j].Azimuth * pi / 180) * cosf(input[j].pitch * pi / 180);
        tmp.vel_y = tmp.velocity * sinf(input[j].Azimuth * pi / 180) * cosf(input[j].pitch * pi / 180);
        tmp.vel_z = tmp.velocity * sinf(input[j].pitch * pi / 180);
        tmp.noise =  input[j].mean_noise;
        output.push_back(tmp);
    }
}

void DataFormatConversion::data_select(std::vector<PointIn> &input, std::vector<PointIn> &output) {
    vector<bool> isvisited(input.size(), false);
    float RangeTH = 7.5;
    float AzimuthTh = 2;
    float PitchTH = 2;
    float DopplerTH = 8;

    float SNR = 0;
    float Range_change = 0;
    float Azimuth_change = 0;
    float Pitch_change = 0;
    float Doppler_change = 0;
    for (int i = 0; i < input.size(); i++) {
        if (isvisited[i] == true) {
            continue;
        }
        isvisited[i] = true;
        output.push_back(input[i]);
        SNR = output[output.size() - 1].SNR;

        for (int j = 0; j < input.size(); j++) {
            if (isvisited[j]) {
                continue;
            }
            Range_change = abs(output[output.size() - 1].Range - input[j].Range);
            Azimuth_change = abs(output[output.size() - 1].Azimuth - input[j].Azimuth);
            Pitch_change = abs(output[output.size() - 1].pitch - input[j].pitch);
            Doppler_change = abs(output[output.size() - 1].Doppler - input[j].Doppler);
            if (Range_change < RangeTH && Azimuth_change < AzimuthTh && Pitch_change < PitchTH) {
                isvisited[j] = true;
                if (input[j].SNR > SNR) {
                    SNR = input[j].SNR;
                    output[output.size() - 1] = input[j];
                }
            }
        }
    }
}

void DataFormatConversion::data_con(std::vector<PointIn> &in_dot_pre, std::vector<PointIn> &in_dot_now,
                                    std::vector<PointIn> &output, bool circle_flag) {

    int upper = 10;
    float range_change = 0;
    float dopp_change = 0;
    float azimuth_change = 0;
    float pitch_change = 0;

    float range_threshold = 5;
    float dopp_threshold = 10;
    float azimuth_threshold = 3;
    float pitch_threshold = 3;
    int N = in_dot_now.size();
    vector<bool> iscon(N, false);//旧点击关联情况
    vector<PointIn> in_dot_temp;

    for (int i = 0; i < in_dot_pre.size(); ++i) {
        PointIn point_tmp;
        bool aos = false;//是否关联
        point_tmp = in_dot_pre[i];
        for (int j = 0; j < in_dot_now.size(); ++j) {
            if (iscon[j]) continue;
            range_change = abs(in_dot_pre[i].Range - in_dot_now[j].Range);
            dopp_change = abs(in_dot_pre[i].Doppler - in_dot_now[j].Doppler);
            azimuth_change = abs(in_dot_pre[i].Azimuth - in_dot_now[j].Azimuth);
            pitch_change = abs(in_dot_pre[i].pitch - in_dot_now[j].pitch);

            bool dopp_diff = ((in_dot_pre[i].Doppler > 32) && (in_dot_now[j].Doppler > 32))
                             || ((in_dot_pre[i].Doppler < 32) && (in_dot_now[j].Doppler < 32));

            if (range_change <= range_threshold && dopp_change <= dopp_threshold &&
                azimuth_change <= azimuth_threshold && pitch_change <= pitch_threshold && dopp_diff) {
                point_tmp = point_tmp.SNR > in_dot_now[j].SNR ? point_tmp : in_dot_now[j];
                aos = true;
                iscon[j] = true;
            }
        }
        if (aos)//关联上了
        {
            point_tmp.con_cnt = in_dot_pre[i].con_cnt + 1;
            in_dot_temp.push_back(point_tmp);
        } else {
            if (point_tmp.con_cnt <= upper) //满足条件进行输出
            {
                output.push_back(point_tmp);
            }
        }

    }
    //将未关联的点存入
    for (int i = 0; i < N; ++i) {
        if (!iscon[i])
            in_dot_temp.push_back(in_dot_now[i]);
    }
    in_dot_pre.clear();
    in_dot_now.clear();
    if (circle_flag) {
        output.insert(output.end(), in_dot_temp.begin(), in_dot_temp.end());
    } else {
        in_dot_pre.insert(in_dot_pre.end(), in_dot_temp.begin(), in_dot_temp.end());
    }
    in_dot_temp.clear();
    return;//凝聚完毕

}
